function X = reconstruction_calibrated(K, xim)

[a b c] = size(xim);



x1 = xim(:,:,1);


i=1;
while ((averageDistance(x1,xim(:,:,i)))<20 && i<c)
     i=i+1;
end;
if (i<c)
    x2 = xim(:,:,i);
else
    x2 = xim(:,:,c);
end;

%x2 = xim(:,:,c);



[F,x1_inl,x2_inl] = ransacF(x1,x2); 
%%%Find Fundamental Matrix by Gold Standard Algorithm
Fgs = gsFundamental(F , x1_inl, x2_inl);
Essential = K'*F*K; 
%Normalise E
[U S V] = svd(Essential);
r = S(1,1);
s = S(2,2);
avg = (r+s) /2;
S(1,1) = avg;
S(2,2) = avg;
S(3,3) = 0;
Essential = U*S*V';


% Find P2
% P2 is find from decomposition of the Essential matrix
% From decomposition of E, the result are 4 possible solution
% From 4 possible solution, find the true solution
[P2_4Possible] = getCameraMatrix(Essential); %Get 4 possible P2
X_TestPoint = [x1_inl(:,1),x2_inl(:,1)]; %X_TestPoint is point correspondence of frame 1 and frame 2 
[P2] = getCorrectCameraMatrix(P2_4Possible, K,K, X_TestPoint); % Get the exact P2

P1 = K*eye(3,4);

%Upgrade P2 obtained from essential matrix
%lsqnonlin is least squares nonlinear minimizing function to find P2 which make the minimum cost function(reprojection error)
h = P2;
P2 = lsqnonlin(@(h)TuneP(x1_inl,P1,x2_inl,h),K*P2);

%Triangulation for image 1 and image 2
X1 = Triangulation(x1_inl,P1,x2_inl,P2);

X = X1;

%{
for j=i+1:c-1 
    if ((averageDistance(x2,xim(:,:,j)))>20)
        x3 = xim(:,:,j);
        
        %compute 3D points for the next two frames
        [F,x22_inl,x3_inl] = ransacF(x2,x3);
        [X2, P3] = addView(x2_inl, x22_inl, x3_inl, X, P2);
        X = [X X2];
        
        %set variables for next loop
        X1 = X2;
        x2_inl = x22_inl;
        x2 = x3;
        P2 = P3;
    end;    
end;  

%}


